Coding the Full Filter

In previous lessons we learned the basis of our filter, tried some example calculations by hand, and implemented critical steps and models for a single time step and vector of sensor observations. In this final coding exercise we will implement the entire filter using the pieces we have already developed for multiple time steps and sensor observations.

Sensor observations are provided in a 2D vector where each inner vector represents the sensor observations, in meters, at a time step.

{{1,7,12,21}, {0,6,11,20}, {5,10,19}, {4,9,18}, {3,8,17}, {2,7,16}, 
{1,6,15}, {0,5,14}, {4,13}, {3,12},{2,11},{1,10},{0,9},{8},{7},{6},{5},
{4},{3},{2},{1},{0}, {}, {}, {}};

Implement the Bayes' localization filter by first initializing priors, then doing the following within each time step:

extract sensor observations
- for each pseudo-position:
  - get the motion model probability
  - determine pseudo ranges
  - get the observation model probability
  - use the motion and observation model probabilities to calculate the posterior probability
- normalize posteriors (see helpers.h for a normalization function)
- update priors (priors --> posteriors)

All tasks are within the main function and are labeled as TODO.

Troubleshooting:

To help troubleshoot print statements have been placed throughout the code below and commented out. Uncommenting these statements will help to follow each step of the filter.

Start Quiz:

main.cpp helpers.h

#include <iostream>
#include <algorithm>
#include <vector>

#include "helpers.h"
using namespace std;


std::vector<float> initialize_priors(int map_size, std::vector<float> landmark_positions,
                                     float control_stdev);

float motion_model(float pseudo_position, float movement, std::vector<float> priors,
                   int map_size, int control_stdev);

//function to get pseudo ranges
std::vector<float> pseudo_range_estimator(std::vector<float> landmark_positions, 
                                          float pseudo_position);

//observation model: calculates likelihood prob term based on landmark proximity
float observation_model(std::vector<float> landmark_positions, std::vector<float> observations, 
                        std::vector<float> pseudo_ranges, float distance_max, 
                        float observation_stdev);


int main() {  

    //set standard deviation of control:
    float control_stdev = 1.0f;

    //meters vehicle moves per time step
    float movement_per_timestep = 1.0f;

    //set observation standard deviation:
    float observation_stdev = 1.0f;

    //number of x positions on map
    int map_size = 25;

    //set distance max
    float distance_max = map_size;

    //define landmarks
    std::vector<float> landmark_positions {3, 9, 14, 23};

    //define observations vector, each inner vector represents a set of observations
    //for a time step
    std::vector<std::vector<float> > sensor_obs {{1,7,12,21}, {0,6,11,20}, {5,10,19}, {4,9,18},
                                    {3,8,17}, {2,7,16}, {1,6,15}, {0,5,14}, {4,13},
                                    {3,12},{2,11},{1,10},{0,9},{8},{7},{6},{5},{4},{3},{2},{1},{0}, 
                                    {}, {}, {}};


    //TODO: initialize priors
    
    //UNCOMMENT TO SEE THIS STEP OF THE FILTER
    /*std::cout << "-----------PRIORS INIT--------------" << endl;

    for (unsigned int p = 0; p < priors.size(); p++){
        std::cout << priors[p] << endl;
    }*/  
    
    //initialize posteriors
    std::vector<float> posteriors(map_size, 0.0);

    //specify time steps
    int time_steps = sensor_obs.size();
    
    //declare observations vector
    std::vector<float> observations;
    
    //cycle through time steps
    for (unsigned int t = 0; t < time_steps; t++){

        //UNCOMMENT TO SEE THIS STEP OF THE FILTER
        /*
        std::cout << "---------------TIME STEP---------------" << endl;
        std::cout << "t = " << t << endl;

        std::cout << "-----Motion----------OBS----------------PRODUCT--" << endl;
        */

        if (!sensor_obs[t].empty()) {
            observations = sensor_obs[t]; 
       } else {
            observations = {float(distance_max)};
       }

        //step through each pseudo position x (i)
        for (unsigned int i = 0; i < map_size; ++i) {
            float pseudo_position = float(i);

            //TODO: get the motion model probability for each x position
            

            //TODO: get pseudo ranges
            

            //TODO: get observation probability
            
            
            //TODO: calculate the ith posterior and pass to posteriors vector
           

            //UNCOMMENT TO SEE THIS STEP OF THE FILTER
            /*std::cout << motion_prob << "\t" << observation_prob << "\t" 
            << "\t"  << motion_prob * observation_prob << endl;
            */

            
        } 
        
        //UNCOMMENT TO SEE THIS STEP OF THE FILTER
        /*std::cout << "----------RAW---------------" << endl;

        for (unsigned int p = 0; p < posteriors.size(); p++) {
            std::cout << posteriors[p] << endl;
        }
        */

        
        //TODO: normalize posteriors (see helpers.h for a helper function)

        //print to stdout
        //std::cout << posteriors[t] <<  "\t" << priors[t] << endl;

        //UNCOMMENT TO SEE THIS STEP OF THE FILTER
        //std::cout << "----------NORMALIZED---------------" << endl;

        //TODO: update priors
    

        //UNCOMMENT TO SEE THIS STEP OF THE FILTER
        /*for (unsigned int p = 0; p < posteriors.size(); p++) {
            std::cout << posteriors[p] << endl;
        }
        */
     

    //print final posteriors vector to stdout
    for (unsigned int p = 0; p < posteriors.size(); p++) {
            std::cout << posteriors[p] << endl;  
        } 
    }

    return 0;
};

//observation model: calculates likelihood prob term based on landmark proximity
float observation_model(std::vector<float> landmark_positions, std::vector<float> observations, 
                        std::vector<float> pseudo_ranges, float distance_max,
                        float observation_stdev) {

    //initialize observation probability:
    float distance_prob = 1.0f;

    //run over current observation vector:
    for (unsigned int z=0; z< observations.size(); ++z) {

        //define min distance:
        float pseudo_range_min;
        
        //check, if distance vector exists:
        if(pseudo_ranges.size() > 0) {
            //set min distance:
            pseudo_range_min = pseudo_ranges[0];
            //remove this entry from pseudo_ranges-vector:
            pseudo_ranges.erase(pseudo_ranges.begin());

        }    

    //no or negative distances: set min distance to a large number:
    else {

        pseudo_range_min = std::numeric_limits<const float>::infinity();

    }

        //estimate the probabiity for observation model, this is our likelihood: 
        distance_prob *= Helpers::normpdf(observations[z], pseudo_range_min,
                                          observation_stdev);
       
    }
    return distance_prob;
}

std::vector<float> pseudo_range_estimator(std::vector<float> landmark_positions,
                                          float pseudo_position) {
    
    //define pseudo observation vector:
    std::vector<float> pseudo_ranges;
            
    //loop over number of landmarks and estimate pseudo ranges:
        for (unsigned int l=0; l< landmark_positions.size(); ++l) {

            //estimate pseudo range for each single landmark 
            //and the current state position pose_i:
            float range_l = landmark_positions[l] - pseudo_position;
            
            //check if distances are positive: 
            if (range_l > 0.0f) {
                pseudo_ranges.push_back(range_l);
            }
        }

    //sort pseudo range vector:
    sort(pseudo_ranges.begin(), pseudo_ranges.end());
    
    return pseudo_ranges;
}

//motion model: calculates prob of being at an estimated position at time t
float motion_model(float pseudo_position, float movement, std::vector<float> priors,
                   int map_size, int control_stdev) {

    //initialize probability
    float position_prob = 0.0f;

    //step over state space for all possible positions x (convolution):
    for (unsigned int j=0; j< map_size; ++j) {
        float next_pseudo_position = float(j);
        
        //distance from i to j
        float distance_ij = pseudo_position-next_pseudo_position;

        //transition probabilities:
        float transition_prob = Helpers::normpdf(distance_ij, movement, 
                            control_stdev);
        
        //estimate probability for the motion model, this is our prior
        position_prob += transition_prob*priors[j];
    }
    return position_prob;
}

//initialize priors assumimg vehicle at landmark +/- 1.0 meters position stdev
std::vector<float> initialize_priors(int map_size, std::vector<float> landmark_positions,
                                     float control_stdev) {
//initialize priors assumimg vehicle at landmark +/- 1.0 meters position stdev

    //set all priors to 0.0
    std::vector<float> priors(map_size, 0.0);

    //set each landmark positon +/1 to 1.0/9.0 (9 possible postions)
    float normalization_term = landmark_positions.size() * (control_stdev * 2 + 1);
    for (unsigned int i = 0; i < landmark_positions.size(); i++){
        int landmark_center = landmark_positions[i];
        priors[landmark_center] = 1.0f/normalization_term;
        priors[landmark_center - 1] = 1.0f/normalization_term;
        priors[landmark_center + 1] = 1.0f/normalization_term;

    }
    return priors;
}

//=================================================================================
// Name        : help_functions.h
// Version     : 2.0.0
// Copyright   : Udacity
//=================================================================================

#ifndef HELP_FUNCTIONS_H_
#define HELP_FUNCTIONS_H_

#include <math.h>
#include <iostream>
#include <vector>
#include <fstream>
#include <sstream>
#include <iomanip>

using namespace std;

class Helpers {
public:

	//definition of one over square root of 2*pi:
	constexpr static float STATIC_ONE_OVER_SQRT_2PI = 1/sqrt(2*M_PI) ;
	float ONE_OVER_SQRT_2PI = 1/sqrt(2*M_PI) ;

	/*****************************************************************************
	 * normpdf(X,mu,sigma) computes the probability function at values x using the
	 * normal distribution with mean mu and standard deviation std. x, mue and 
	 * sigma must be scalar! The parameter std must be positive. 
	 * The normal pdf is y=f(x;mu,std)= 1/(std*sqrt(2pi)) e[ -(x−mu)^2 / 2*std^2 ]
	*****************************************************************************/
	static float normpdf(float x, float mu, float std) {
	    return (STATIC_ONE_OVER_SQRT_2PI/std)*exp(-0.5*pow((x-mu)/std,2));
	}

	//static function to normalize a vector:
	static std::vector<float> normalize_vector(std::vector<float> inputVector){

		//declare sum:
		float sum = 0.0f;

		//declare and resize output vector:
		std::vector<float> outputVector ;
		outputVector.resize(inputVector.size());

		//estimate the sum:
		for (unsigned int i = 0; i < inputVector.size(); ++i) {
			sum += inputVector[i];
		}

		//normalize with sum:
		for (unsigned int i = 0; i < inputVector.size(); ++i) {
			outputVector[i] = inputVector[i]/sum;
		}

		//return normalized vector:
		return outputVector;
	}
	
};

#endif /* HELP_FUNCTIONS_H_ */